
import time
from math import *
import serial
import minimalmodbus as mm
import io
import libscrc
import _thread
import threading
import numpy as np
from Hardware.BaseFtSensor import BaseFt

class RobotiqFt300(BaseFt):
    def __init__(self):
        self.setZeroForce=False
        self.zeroRef0 = [0, 0, 0, 0, 0, 0]
        self.isRunning=True
        BAUDRATE = 19200
        BYTESIZE = 8
        PARITY = "N"
        STOPBITS = 1
        TIMEOUT = 1
        PORTNAME = "COM4"
        SLAVEADDRESS = 9
        self.ser = serial.Serial(port=PORTNAME, baudrate=BAUDRATE, bytesize=BYTESIZE, parity=PARITY, stopbits=STOPBITS,
                            timeout=TIMEOUT)
        packet = bytearray()
        sendCount = 0
        while sendCount < 50:
            packet.append(0xff)
            sendCount = sendCount + 1
        self.ser.write(packet)
        self.ser.close()
        del packet
        del sendCount
        del self.ser

        # Communication setup
        mm.BAUDRATE = BAUDRATE
        mm.BYTESIZE = BYTESIZE
        mm.PARITY = PARITY
        mm.STOPBITS = STOPBITS
        mm.TIMEOUT = TIMEOUT

        # Create FT300 object
        ft300 = mm.Instrument(PORTNAME, slaveaddress=SLAVEADDRESS)
        ft300.close_port_after_each_call = True

        # Uncomment to see binary messages for debug
        # ft300.debug=True
        # ft300.mode=mm.MODE_RTU

        # Write 0x0200 in 410 register to start streaming
        ###############################################
        registers = ft300.write_register(410, 0x0200)

        del ft300

        ############################
        # Open serial connection
        ############################

        self.ser = serial.Serial(port=PORTNAME, baudrate=BAUDRATE, bytesize=BYTESIZE, parity=PARITY, stopbits=STOPBITS,
                            timeout=TIMEOUT)

        ############################
        # Initialize stream reading
        ############################

        # Bytes use to itendify the beginning of the serial message
        self.STARTBYTES = bytes([0x20, 0x4e])

        # Read serial buffer until founding the bytes [0x20,0x4e]
        # First serial reading.
        # This message in uncomplete in most cases so it is ignored.
        data = self.ser.read_until(self.STARTBYTES)

        # Second serial reading.
        # This message is use to make the zero of the sensor.
        data = self.ser.read_until(self.STARTBYTES)
        # convert from byte to bytearray
        dataArray = bytearray(data)
        # Delete the end bytes [0x20,0x4e] and place it at the beginning of the bytearray
        dataArray = self.STARTBYTES + dataArray[:-2]
        # Check if the serial message have a valid CRC
        if self.crcCheck(dataArray) is False:
            raise Exception("CRC ERROR: Serial message and the CRC does not match")

        # 初始化清零
        #self.forceFromSerialMessage(dataArray,isZero=True)

        # Program variables
        ##################

        # Number of received messages
        self.nbrMessages = 0
        # Data rate frequency in Hz
        frequency = 0
        self.forceTorque=[0,0,0,0,0,0]
        self.startTime = time.time()
        #_thread.start_new_thread(self.start(),())
        self.thread=threading.Thread(target=self.start)
        self.thread.start()

        print("FT init")

    def start(self):
        while self.isRunning:
            # Read serial message
            ####################
            data = self.ser.read_until(self.STARTBYTES)
            # convert from byte to bytearray
            dataArray = bytearray(data)
            # Delete the end bytes [0x20,0x4e] and place it at the beginning of the bytearray
            dataArray = self.STARTBYTES + dataArray[:-2]

            # calulate force and torque form serial message
            #############################################
            self.forceTorque = self.forceFromSerialMessage(dataArray, self.setZeroForce)

            # CRC validation
            ################
            if self.crcCheck(dataArray) is False:
                #raise Exception("CRC ERROR: Serial message and the CRC does not match")
                print("CRC ERROR: Serial message and the CRC does not match")
                continue

            # Frequency
            ###############
            # Update message counter
            self.nbrMessages += 1
            # Update timer
            elapsedTime = time.time() - self.startTime+0.00000001
            # Calculate average frequency
            frequency = round(self.nbrMessages / elapsedTime)

            # Data printing
            ###############
            if(self.setZeroForce==True):
                self.setZeroForce=False
            #print("F: ", frequency, "Hz - force Vector : ", self.forceTorque,self.setZeroForce)

    def GetForce(self):
        #增加判断是否连接，如果不连接，返回错误
        # print(self.forceTorque.copy())
        return self.forceTorque.copy()

    def FtZero(self):
        print("FT清零")
        self.setZeroForce=True

        # force=self.GetForce()
        # self.zeroRef=np.sum([force, self.zeroRef], axis=0)
        # print("zero force=",self.zeroRef)

    def DisConnect(self):
        self.isRunning=False
    def forceFromSerialMessage(self,serialMessage,isZero=False):
        """Return a list with force and torque values [Fx,Fy,Fz,Tx,Ty,Tz] correcponding to the dataArray

        Parameters
        ----------
        serialMessage:
          bytearray which contents the serial message send by the FT300.
          [0x20,0x4e,LSBdata1,MSBdata2,...,LSBdata6,MSBdata6,crc1,crc2]
          Check FT300 manual for details.
        zeroRef:
          list with force and torque values [Fx,Fy,Fz,Tx,Ty,Tz] use the set the zero reference of the sensor.

        Return
        ------
        forceTorque:
          list with force and torque values [Fx,Fy,Fz,Tx,Ty,Tz] correcponding to the dataArray
        """
        # Initialize variable
        #1,解析力数值
        forceTorque = [0, 0, 0, 0, 0, 0]
        forceTorque[0] = round(int.from_bytes(serialMessage[2:4], byteorder='little', signed=True) / 100 , 2)
        forceTorque[1] = round(int.from_bytes(serialMessage[4:6], byteorder='little', signed=True) / 100 , 2)
        forceTorque[2] = round(int.from_bytes(serialMessage[6:8], byteorder='little', signed=True) / 100 , 2)
        forceTorque[3] = round(int.from_bytes(serialMessage[8:10], byteorder='little', signed=True) / 1000 ,2)
        forceTorque[4] = round(int.from_bytes(serialMessage[10:12], byteorder='little', signed=True) / 1000 , 2)
        forceTorque[5] = round(int.from_bytes(serialMessage[12:14], byteorder='little', signed=True) / 1000 , 2)

        #2,如果需要清零，把基准设置为当前值
        if(isZero==True):
            self.zeroRef0 = forceTorque.copy()

        #3，把数值减去基准
        forceTorque[0]-=self.zeroRef0[0]
        forceTorque[1]-=self.zeroRef0[1]
        forceTorque[2]-=self.zeroRef0[2]
        forceTorque[3]-=self.zeroRef0[3]
        forceTorque[4]-=self.zeroRef0[4]
        forceTorque[5]-=self.zeroRef0[5]
        forceTorque=np.round(forceTorque,2)
        return forceTorque


    def forceFromSerialMessageold(self,serialMessage, zeroRef=[0, 0, 0, 0, 0, 0]):
        """Return a list with force and torque values [Fx,Fy,Fz,Tx,Ty,Tz] correcponding to the dataArray

        Parameters
        ----------
        serialMessage:
          bytearray which contents the serial message send by the FT300.
          [0x20,0x4e,LSBdata1,MSBdata2,...,LSBdata6,MSBdata6,crc1,crc2]
          Check FT300 manual for details.
        zeroRef:
          list with force and torque values [Fx,Fy,Fz,Tx,Ty,Tz] use the set the zero reference of the sensor.

        Return
        ------
        forceTorque:
          list with force and torque values [Fx,Fy,Fz,Tx,Ty,Tz] correcponding to the dataArray
        """
        # Initialize variable
        forceTorque = [0, 0, 0, 0, 0, 0]

        # converte bytearray values to integer. Apply the zero offset and round at 2 decimals
        forceTorque[0] = round(int.from_bytes(serialMessage[2:4], byteorder='little', signed=True) / 100 - zeroRef[0],
                               2)
        forceTorque[1] = round(int.from_bytes(serialMessage[4:6], byteorder='little', signed=True) / 100 - zeroRef[1],
                               2)
        forceTorque[2] = round(int.from_bytes(serialMessage[6:8], byteorder='little', signed=True) / 100 - zeroRef[2],
                               2)
        forceTorque[3] = round(int.from_bytes(serialMessage[8:10], byteorder='little', signed=True) / 1000 - zeroRef[3],
                               2)
        forceTorque[4] = round(
            int.from_bytes(serialMessage[10:12], byteorder='little', signed=True) / 1000 - zeroRef[4], 2)
        forceTorque[5] = round(
            int.from_bytes(serialMessage[12:14], byteorder='little', signed=True) / 1000 - zeroRef[5], 2)
        #print("zeroRef=",zeroRef)
        return forceTorque

    def crcCheck(self,serialMessage):
        """Check if the serial message have a valid CRC.

        Parameters
        -----------
        serialMessage:
          bytearray which contents the serial message send by the FT300.
          [0x20,0x4e,LSBdata1,MSBdata2,...,LSBdata6,MSBdata6,crc1,crc2]
          Check FT300 manual for details.

        Return
        ------
        checkResult:
          bool, return True if the message have a valid CRC and False if not.
        """
        checkResult = False

        # CRC from serial message
        crc = int.from_bytes(serialMessage[14:16], byteorder='little', signed=False)
        # calculated CRC
        crcCalc = libscrc.modbus(serialMessage[0:14])

        if crc == crcCalc:
            checkResult = True

        return checkResult
